#include "StdAfx.h"

#include "MNMPathfinder.h"
#include "Navigation/NavigationSystem/NavigationSystem.h"
#include "Navigation/MNM/MeshGrid.h"
#include "DebugDrawContext.h"
// Francesco TODO: Uncomment when integrating AIBubblesSystem
//#include "AIBubblesSystem/IAIBubblesSystem.h"
#include "Navigation/PathHolder.h"

//#pragma optimize("", off)
//#pragma inline_depth(0)

struct WayQueryRequestAICharacter : public MNM::MeshGrid::WayQueryRequest
{
	WayQueryRequestAICharacter(const IAIPathAgent* pRequesterAgent, MNM::TriangleID from, MNM::TriangleID to, const MNM::OffMeshNavigation& offMeshNavigation, MNM::AStarOpenList& openList)
		: MNM::MeshGrid::WayQueryRequest(from, to, offMeshNavigation, openList)
		, m_pRequester(pRequesterAgent) 
	{

	}
	virtual ~WayQueryRequestAICharacter() {}

	virtual bool CanUseOffMeshLink(const MNM::OffMeshLinkID linkID) const
	{
		return GetOffMeshNavigation().CanUseLink(m_pRequester, linkID);
	}

private:
	const IAIPathAgent* m_pRequester;
};

//////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////

CMNMPathfinder::CMNMPathfinder()
{
}

CMNMPathfinder::~CMNMPathfinder()
{
}

void CMNMPathfinder::Reset()
{
	m_processingRequestData = QueuedRequest();
	m_processingRequest.Reset();
	m_AStarOpenList.Reset();

	m_requestedPathsQueue.clear();
	m_navPath.Clear("CMNMPathfinder::Reset");
}

MNMQueuedPathID CMNMPathfinder::RequestPathTo(IAIPathAgent* pRequester, const MNMPathRequest& request)
{
	//////////////////////////////////////////////////////////////////////////
	// Validate requester
	if (!pRequester || (pRequester->GetPathAgentType() != AIOBJECT_ACTOR && pRequester->GetPathAgentType() != AIOBJECT_VEHICLE))
	{
		AIWarning("[CMNMPathfinder::QueuePathRequest] No agent specified. A NavigationTypeID is needed to find the appropriate mesh.");
		return 0;
	}

	//////////////////////////////////////////////////////////////////////////
	// Validate Agent Type
	const char *actorName = pRequester->GetPathAgentName();

	const IEntity* pEntity = pRequester->GetPathAgentEntity();
	EntityId entityId = pEntity ? pEntity->GetId() : 0;

	if (!request.agentTypeID)
	{
		AIWarning("[CMNMPathfinder::QueuePathRequest] Request from agent %s has no NavigationType defined.", actorName);
		return 0;
	}

	//////////////////////////////////////////////////////////////////////////
	// Validate callback
	if (!request.resultCallback)
	{
		AIWarning("[CMNMPathfinder::QueuePathRequest] Agent %s does not provide a result Callback", actorName);
		return 0;
	}

	//////////////////////////////////////////////////////////////////////////
	// Validate start/end locations
	const NavigationMeshID meshID = gAIEnv.pNavigationSystem->GetEnclosingMeshID(request.agentTypeID, request.startLocation);

	if (!meshID)
	{
		// Francesco TODO: Uncomment when integrating AIBubblesSystem
		//AIQueueBubbleMessage("CMNMPathfinder::RequestPathTo-NoMesh", entityId, 
		//	"I m not inside a Navigation area for my navigation type.",
		//	eBNS_Balloon|eBNS_Log|eBNS_BlockingPopup);
		return 0;
	}

	return m_requestedPathsQueue.push_back(QueuedRequest(pRequester, request.agentTypeID, request));
}

void CMNMPathfinder::CancelPathRequest( MNMQueuedPathID requestId )
{
	//Check if it is the current one
	if (m_processingRequest.IsValid() && m_processingRequest.queuedID == requestId)
	{
		m_processingRequest.Reset();
	}

	//Check if in the queue
	if (m_requestedPathsQueue.has(requestId))
	{
		m_requestedPathsQueue.erase(requestId);
	}
}

void CMNMPathfinder::Update()
{
	bool continueUpdate = true;

	m_AStarOpenList.SetFrameTimeQuota(gAIEnv.CVars.MNMPathFinderQuota);

	m_AStarOpenList.OnUpdateStart(m_requestedPathsQueue.size());

	//////////////////////////////////////////////////////////////////////////
	// Check if still computing last one
	if (m_processingRequest.IsValid())
	{
		continueUpdate = ProcessPathRequest();
	}

	//////////////////////////////////////////////////////////////////////////
	// Continue with the rest of the queue if possible
	while(!m_requestedPathsQueue.empty() && continueUpdate)
	{
		const MNMQueuedPathID nextPathRequestID = m_requestedPathsQueue.front_id();
		const QueuedRequest nextPathRequest = m_requestedPathsQueue.front();
		m_requestedPathsQueue.pop_front();

		if (SetupForNextPathRequest(nextPathRequestID, nextPathRequest))
		{
			continueUpdate = ProcessPathRequest();
		}
		else
		{
			PathRequestFailed(nextPathRequestID, nextPathRequest);
		}
	}

	if (gAIEnv.CVars.MNMPathFinderDebug)
	{
		DebugStatistics();
	}
}


void CMNMPathfinder::OnNavigationMeshChanged( const NavigationMeshID meshId, const MNM::TileID tileId )
{
	//////////////////////////////////////////////////////////////////////////
	/// Check if the current processed request could be affected by the change

	if (!m_processingRequest.IsValid())
		return;

	if (m_processingRequest.meshID != meshId)
		return;

	if (!m_AStarOpenList.TileWasVisited(tileId))
		return;

	//////////////////////////////////////////////////////////////////////////
	/// Re-start current request for next update

	MNMQueuedPathID requestId = m_processingRequest.queuedID;
	QueuedRequest requestParams = m_processingRequestData;

	if(!SetupForNextPathRequest(requestId, requestParams))
	{
		PathRequestFailed(requestId, requestParams);
	}
}

bool CMNMPathfinder::SetupForNextPathRequest( MNMQueuedPathID requestID, const QueuedRequest& request )
{
	m_processingRequest.Reset();

	//////////////////////////////////////////////////////////////////////////
	// Validate start/end locations
	const NavigationMeshID meshID = gAIEnv.pNavigationSystem->GetEnclosingMeshID(request.agentTypeID, request.requestParams.startLocation);

	if (!meshID)
	{
		AIWarning("[CMNMPathfinder::SetupForNextPathRequest] Agent %s is not inside a navigation volume.", request.pRequester->GetPathAgentName());
		return false;
	}

	const NavigationMesh& mesh = gAIEnv.pNavigationSystem->GetMesh(meshID);
	const MNM::MeshGrid& grid = mesh.grid;

	const MNM::MeshGrid::Params& gridParams = grid.GetParams();
	const MNM::vector3_t origin = MNM::vector3_t(MNM::real_t(gridParams.origin.x), MNM::real_t(gridParams.origin.y),MNM::real_t(gridParams.origin.z));

	const uint16 agentRadiusUnits = gAIEnv.pNavigationSystem->GetAgentRadiusInVoxelUnits(request.agentTypeID);
	const uint16 agentHeightUnits = gAIEnv.pNavigationSystem->GetAgentHeightInVoxelUnits(request.agentTypeID);
	
	const MNM::vector3_t startLocation(MNM::real_t(request.requestParams.startLocation.x), MNM::real_t(request.requestParams.startLocation.y), MNM::real_t(request.requestParams.startLocation.z));
	const MNM::vector3_t endLocation(MNM::real_t(request.requestParams.endLocation.x), MNM::real_t(request.requestParams.endLocation.y), MNM::real_t(request.requestParams.endLocation.z));
	const Vec3 voxelSize = grid.GetParams().voxelSize;
	const MNM::real_t horizontalRange = MNMUtils::CalculateMinHorizontalRange(agentRadiusUnits, voxelSize.x);
	const MNM::real_t verticalRange = MNMUtils::CalculateMinVerticalRange(agentHeightUnits, voxelSize.z);

	AgentType agentTypeProperties;
	const bool arePropertiesValid = gAIEnv.pNavigationSystem->GetAgentTypeProperties(request.agentTypeID, agentTypeProperties);
	assert(arePropertiesValid);
	const uint16 zOffsetMultiplier = min(uint16(2), agentTypeProperties.settings.heightVoxelCount); 
	const MNM::real_t verticalUpwardRange = arePropertiesValid ? MNM::real_t( zOffsetMultiplier * agentTypeProperties.settings.voxelSize.z) : MNM::real_t(.0f);

	Vec3 safeStartLocation(request.requestParams.startLocation);
	MNM::TriangleID triangleStartID;
	if (!(triangleStartID = grid.GetTriangleAt(startLocation - origin, verticalRange, verticalUpwardRange)))
	{
		if (!(triangleStartID = grid.GetClosestTriangle( startLocation - origin, verticalRange, horizontalRange)))
		{
			AIWarning("Navigation system couldn't figure out where the start point for agent %s was.", request.pRequester->GetPathAgentName());
			return false;
		}
		else
		{
			MNM::vector3_t v0, v1, v2;
			grid.GetVertices(triangleStartID, v0, v1, v2);
			safeStartLocation = TriangleCenter((v0 + origin).GetVec3(), (v1 + origin).GetVec3(), (v2 + origin).GetVec3());
		}
	}

	Vec3 safeEndLocation(request.requestParams.endLocation);
	MNM::TriangleID triangleEndID = grid.GetTriangleAt(endLocation - origin, verticalRange, verticalRange);
	if (!triangleEndID)
	{
		MNM::vector3_t closest;
		triangleEndID = grid.GetClosestTriangle(endLocation - origin , verticalRange, horizontalRange, NULL, &closest);
		if (triangleEndID)
		{
			safeEndLocation = closest.GetVec3();
		}
		else
		{
			AIWarning("Navigation system couldn't figure out where the destination point for agent %s was.", request.pRequester->GetPathAgentName());
			return false;
		}
	}

	m_processingRequest.pRequester = request.pRequester;
	m_processingRequest.meshID = meshID;
	m_processingRequest.fromTriangleID = triangleStartID;
	m_processingRequest.toTriangleID = triangleEndID;
	m_processingRequest.queuedID = requestID;
	m_processingRequest.requestParams = request.requestParams;
	m_processingRequest.requestParams.startLocation = safeStartLocation;
	m_processingRequest.requestParams.endLocation = safeEndLocation;

	m_processingRequestData = request;

	m_AStarOpenList.SetUpForPathSolving(mesh.grid.GetTriangleCount(), triangleStartID, startLocation);

	return true;
}

bool CMNMPathfinder::ProcessPathRequest()
{
	assert(m_processingRequest.IsValid());

	const NavigationMesh& mesh = gAIEnv.pNavigationSystem->GetMesh(m_processingRequest.meshID);
	const MNM::MeshGrid& grid = mesh.grid;
	const MNM::OffMeshNavigation& meshOffMeshNav = gAIEnv.pNavigationSystem->GetOffMeshNavigationManager()->GetOffMeshNavigationForMesh(m_processingRequest.meshID);

	const size_t MaxWaySize = 512;
	MNM::WayTriangleData outputWay[MaxWaySize];

	WayQueryRequestAICharacter inputParams(m_processingRequest.pRequester, m_processingRequest.fromTriangleID, m_processingRequest.toTriangleID, meshOffMeshNav, m_AStarOpenList);
	const MNM::MeshGrid::WayQueryResult result = grid.FindWay(inputParams, &outputWay[0], MaxWaySize);

	CPathHolder<PathPointDescriptor> outputPath;

	if (result.Finished())
	{
		m_navPath.Clear("CMNMPathfinder::ProcessPathRequest");

		PathPointDescriptor navPathStart(IAISystem::NAV_UNSET, m_processingRequest.requestParams.startLocation);
		PathPointDescriptor navPathEnd(IAISystem::NAV_UNSET, m_processingRequest.requestParams.endLocation);

		const MNM::MeshGrid::Params& gridParams = grid.GetParams();
		const MNM::vector3_t origin = MNM::vector3_t(MNM::real_t(gridParams.origin.x), MNM::real_t(gridParams.origin.y),MNM::real_t(gridParams.origin.z));

		size_t waySize = result.GetWaySize();

		for (size_t i = 0; i < waySize; ++i)
		{
			// Using the edge-midpoints of adjacent triangles to build the path.
			if (i > 0)
			{
				Vec3 edgeMidPoint;
				if (grid.CalculateMidEdge(outputWay[i-1].triangleID, outputWay[i].triangleID, edgeMidPoint))
				{
					outputPath.PushFront(PathPointDescriptor(IAISystem::NAV_UNSET, edgeMidPoint + origin.GetVec3()));
				}
			}

			if (!outputWay[i].offMeshLinkID)
			{
				if ((i < waySize-1) && (outputWay[i+1].offMeshLinkID))
				{
					MNM::vector3_t a, b, c;
					grid.GetVertices(outputWay[i].triangleID, a, b, c);

					outputPath.PushFront(PathPointDescriptor(IAISystem::NAV_UNSET, 
						TriangleCenter((a + origin).GetVec3(),(b + origin).GetVec3(),(c + origin).GetVec3())
						));
				}
			}
			else
			{
				const MNM::OffMeshLink* pOffMeshLink = meshOffMeshNav.GetObjectLinkInfo(outputWay[i].offMeshLinkID);
				assert(pOffMeshLink);

				if (pOffMeshLink)
				{
					PathPointDescriptor pathPoint(IAISystem::NAV_SMARTOBJECT, pOffMeshLink->GetStartPosition());
					pathPoint.soMNMData.meshID = m_processingRequest.meshID;
					pathPoint.soMNMData.offMeshLinkID = outputWay[i].offMeshLinkID;
					outputPath.PushFront(pathPoint);
				}
			}
		}
		const bool pathFound = (result.GetWaySize() != 0);

		SAIEVENT event;
		event.bPathFound = pathFound;
		if (pathFound)
		{

			//Insert start/end locations in the path
			outputPath.PushBack(navPathEnd);
			outputPath.PushFront(navPathStart);
			if(gAIEnv.CVars.BeautifyPath)
			{
				outputPath.PullPathOnNavigationMesh(m_processingRequest.meshID,gAIEnv.CVars.PathStringPullingIterations,&outputWay[0],result.GetWaySize());
			}
			 
			outputPath.FillNavPath(m_navPath);

			const MNMPathRequest& requestParams = m_processingRequest.requestParams;
			SNavPathParams pparams(requestParams.startLocation, requestParams.endLocation, Vec3(0.0f, 0.0f, 0.0f), requestParams.endDirection, requestParams.forceTargetBuildingId,
				requestParams.allowDangerousDestination, requestParams.endDistance);
			pparams.meshID = m_processingRequest.meshID;

			m_navPath.SetParams(pparams);
			m_navPath.SetEndDir(requestParams.endDirection);

			event.vPosition = m_navPath.GetLastPathPos(ZERO);
		}

		m_processingRequest.requestParams.resultCallback(m_processingRequest.queuedID, event);

		m_processingRequest.Reset();

		m_AStarOpenList.PathSolvingDone();
	}

	return result.Finished();
}

void CMNMPathfinder::PathRequestFailed( MNMQueuedPathID requestID, const QueuedRequest& request )
{
	m_navPath.Clear("CMNMPathfinder::PathRequestFailed");

	SAIEVENT event;
	event.bPathFound = false;
	request.requestParams.resultCallback(requestID, event);
}

void CMNMPathfinder::DebugStatistics()
{
	stack_string text;

	MNM::AStarContention::ContentionStats stats = m_AStarOpenList.GetContentionStats();
	
	text.Format(
		"MNMPathFinder - Frame time quota (%f ms)\n"
		"---------\n"
		"Queue Size:  Current - %d / Maximum - %d\n"
		"AStar steps: Average - %d / Maximum - %d\n"
		"AStar time:  Average - %.4f ms / Maximum - %.4f ms", 
		stats.frameTimeQuota,
		stats.queueSize,
		stats.peakQueueSize,
		stats.averageSearchSteps,
		stats.peakSearchSteps,
		stats.averageSearchTime,
		stats.peakSearchTime);

	const ColorF color = Col_White;
	gEnv->pRenderer->Draw2dLabel(400.f, 40.f, 1.4f, &color.r, false, "%s", text.c_str());
}
